perm filename ARMSOL.OLD[AL,HE] blob
sn#528537 filedate 1981-03-04 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00027 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00004 00002 ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS
C00006 00003 SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM
C00009 00004 ["SOLVE" - CHECK IF ARM OR HAND]
C00012 00005 ["SOLVE" - JOINT 1]
C00015 00006 ["SOLVE" - JOINTS 2 & 3]
C00017 00007 ["SOLVE" - JOINT 5]
C00020 00008 ["SOLVE" - DEGENERATE SOLUTION AND JOINT 4]
C00024 00009 ["SOLVE" - JOINT 6]
C00027 00010 ["SOLVE" LOCAL STORAGE AREA]
C00029 00011 UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES
C00033 00012 ["UPDATE"- HAND OR T6]
C00035 00013 JACOB - COMPUTES JACOBIAN MATRIX
C00036 00014 ["JACOB" - FORM CT0,CT3,CT6]
C00039 00015 ["JACOB" - POSITION VECTORS, AXES OF ROTATION]
C00042 00016 ["JACOB" - CROSS PRODUCTS, CLEAN UP]
C00044 00017 T0336 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 1-3 AND 4-6
C00047 00018 T6330 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 6-4 AND 3-0
C00049 00019 R03 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 1-3
C00051 00020 R36 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 4-6
C00054 00021 INVERT - INVERTS A TRANSFORMATION STORED BY COLUMNS
C00055 00022 MATMUL - PERFORMS THE OPERATION T0 ← T1*T2
C00058 00023 MULT - PERFORMS THE OPERATION A0 ← A1*A2
C00060 00024 CROSS - VECTOR CROSS PRODUCT OPERATION V0 ← V1 X V2
C00062 00025 LOCAL STORAGE AREA
C00064 00026 PHYSICAL CONSTANTS FOR YELLOW AND BLUE ARMS
C00066 00027 [BLUE ARM TABLE OF CONSTANTS]
C00068 ENDMK
C⊗;
;ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS
.TITLE ARMSOL
;THE FOLLOWING PROGRAMS ARE USED TO COMPUTE THE JOINT ANGLES FOR THE
;SCHEINMAN MANIPULATORS GIVEN A REQUIRED TRANSFORM OR VISE VERSA.
;ALSO INCLUDED IS A PROGRAM FOR COMPUTING THE REACTION TORQUES AT THE
;JOINTS FOR A GIVEN FORCE AND MOMENT COUPLE AT THE HAND.
;THE THEORETICAL BASIS FOR THE KINEMATIC SOLUTIONS IMPLEMENTED IN
;THESE PROGRAMS IS PRESENTED IN STANFORD ARTIFICIAL INTELLIGENCE
;LABORATORY HAND/EYE GROUP PROGRESS REPORT, DEC 1975 THRU JULY 1976.
;WHEREVER POSSIBLE, THE VARIABLE NAMES USED IN THAT DOCUMENT AND IN
;THE FOLLOWING PROGRAMS ARE THE SAME.
;THE FOLLOWING MECHANISM BITS INDICATE WHICH PHYSICAL DEVICE IS BEING
;USED:
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
YELHND ==2 ;YELLOW HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND ==10 ;BLUE HAND
;THE FOLLOWING BITS ARE USED BY THE FORCE SENSING ROUTINES TO INDICATE
;IF THE FORCE TRANSFORMATION IS IN TABLE OR HAND COORDINATES.
FTABLE ==400 ;TABLE COORDINATES
FHAND ==0 ;HAND COORDINATE SYSTEM
;TRANSFORMATION DEFINITIONS
T11==0
T21==4
T31==10
T12==14
T22==20
T32==24
T13==30
T23==34
T33==40
T14==44
T24==50
T34==54
;SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM
;GIVEN A TRANSFORM "T", THE REQUIRED JOINT ANGLES ARE DETERMINED IN DEGREES.
;IF ANY JOINT ANGLE IS OUTSIDE OF THE PERMITTED RANGE OF MOVEMENT, THE STOP
;LIMIT ANGLE CLOSEST TO THE PREVIOUS JOINT ANGLES COMPUTED IS RETURNED.
;IN THE DETERMINATION OF JOINT ANGLE 4, IF THE REQUIRED CHANGE IN ANGLE IS
;GREATER THAN 90 DEGRESS AND LESS THAN 180, THE HAND IS FLIPPED OVER AND THE
;JOINT ANGLE CHANGE BECOMES THETA = THETA-90. ON COMPLETION OF EXECUTION,
;REGISTER R0 CONTAINS BITS INDICATING THOSE JOINTS FOR WHICH NO SOLUTION
;COULD BE FOUND WITHIN THE RANGE OF JOINT MOTION. A SAMPLE CALLING SEQUENCE
;TO "SOLVE" FOLLOWS:
;
; MOV #T,R0 ;LOAD ADDRESS OF TRANSFORM "T"
; MOV #THETA,R1 ;PTR TO A TABLE CONTAINING POINTERS TO THE
; ; JOINT ANGLES FOR ALL EXISTING ARM JOINTS,
; ; I.E., THE TABLE MUST HAVE 14 ELEMENTS.
; MOV #MECHSM,R2 ;CONSTANT INDICATING WHICH ARM TO USE
; JSR PC,SOLVE ;CALLED USING PC
; TST R0 ;CHECK FOR NUMBER OF NON-EXACT SOLUTIONS
;
;IF THE BLUE OR YELLOW HAND IS SPECIFIED AS THE MECHANISM, THIS ROUTINE
;PUTS THE VALUE POINTED TO BY "T" INTO ITS APPROPRIATE SPOT IN THE "THETA"
;ARRAY. ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT
;REPRESENATATION.
;EXECUTION TIME: 2200 MICRO SECONDS
;REGISTERS USED:
;
; R0,R1 PASS ARGUMENTS AND ARE ALTER BY "SOLVE",R2 PASSES ARGUMENTS AND IS
; UNALTERED.
; AC0, AC1, AC2, AND AC3 ARE GARBAGED
;THE OUT OF RANGE BITS THAT ARE RETURNED IN R0 ARE DEFINED AS FOLLOWS:
JT1== 1 ;JOINT 1
JT3== 4 ; " 3
JT4== 10 ; " 4
JT5== 20 ; " 5
JT6== 40 ; " 6
; ["SOLVE" - CHECK IF ARM OR HAND]
CODE
SOLVE: CLR EXACTS ;ASSUME EXACT SOLUTION FOUND
MOV R5,-(SP) ;SAVE REGISTER
BIT #BLUHND+YELHND,R2 ;CHECK IF A HAND IS SPECIFIED
BEQ 1$ ;BRANCH IF NOT
LDF (R0),AC0 ;GET THE REQUESTED HAND OPENING
ADD #12.,R1 ;POINT TO THE YELLOW HAND CELL
BIT #BLUHND,R2 ;CHECK WHICH HAND
BEQ .+6
ADD #14.,R1 ;POINT TO BLUE HAND
STF AC0,@(R1) ;SAVE THE HAND OPENING
JMP SOLDNE ;ELSE EXIT IMMEDIATELY
1$: MOV #YCON,R5 ;MUST BE AN ARM, GET PTR TO YELLOW ARM CONSTANTS
BIT #YELARM,R2 ;SWITCH CONSTANTS IF BLUE ARM
BNE 2$
MOV #BCON,R5
ADD #14.,R1 ;POINT TO BLUE ARM JT. ANGLES
;COMPUTE POSITION OF THE END OF THE BOOM: TX,TY,TZ. ALSO COMPUTE
;SQRT(TX**2+TY**2-S2**2) FOR LATER
2$: LDF S6(R5),AC1 ;GET OFFSET OF JT. 6: S6
NEGF AC1
LDF T13(R0),AC0 ;TX ← T14 - T13*S6 - X BASE POS.
MULF AC1,AC0
SUBF BASEX(R5),AC0
ADDF T14(R0),AC0
STF AC0,TX ;SAVE TX
MULF AC0,AC0 ;AC0 ← TX**2
LDF T23(R0),AC2 ;TY ← T24 - T23*S6 - Y BASE COORD.
MULF AC1,AC2
SUBF BASEY(R5),AC2
ADDF T24(R0),AC2
STF AC2,TY ;SAVE TY
MULF AC2,AC2 ;AC0 ← TX**2 + TY**2
ADDF AC2,AC0
MULF T33(R0),AC1 ;TZ ← T34 - T33*S6
ADDF T34(R0),AC1
STF AC1,TZ ;SAVE TZ
SUBF S22(R5),AC0 ;AC0 ← TX**2 + TY**2 - S2**2
CFCC ;CHECK IF RESULT > 0
BGT 3$
CLRF AC0 ;IF <0 SET TO 0 AND INDICATE ERROR
BIS #JT1,EXACTS ;THIS WILL CAUSE JT 1 TO BE WRONG
3$: JSR PC,SQRTF
NEGF AC0
STF AC0,SQRTXY ;SAVE -SQRT(TX**2+TY**2-S2**29
; ["SOLVE" - JOINT 1]
;COMPUTE JOINT 1 ANGLE
SUBF TX,AC0 ;TAN TH1/2= (-TX-(TX↑2+TY↑2-S2↑2)↑.5)/(S2+TY)
LDF TY,AC1 ;INDETERMINANT? IS S2+TY = 0 ?
ADDF S2(R5),AC1
STF AC1,AC2 ;COMPARE TO .001
ABSF AC2
CMPF C001,AC2
CFCC
BLT 5$ ;BRANCH IF DENOMINATOR NOT ZERO
TST TX ;ELSE CHECK IF INDETERMINATE OR SPECIAL CASE
BLT 4$ ;BRANCH IF INDETERMINATE FORM OF EQU. NECESSARY
LDF C180,AC0 ;ELSE THETA 1 = 180 DEGREES
BR JT1CHK ;NOW GO CHECK LIMITS
4$: LDF TY,AC0 ;TAN TH1/2 = TY/TX
LDF TX,AC1
5$: TSTF AC1 ;-90 < TH1/2 < 90
CFCC
BGE JT1CPU
NEGF AC0
NEGF AC1
JT1CPU: JSR PC,ATAN2 ;COMPUTE THETA 1/2
MULF #40400,AC0 ;THETA 1
JT1CHK: CMP #YELARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE 1$ ;BRANCH IF BLUE ARM
CMPF STOP1+4(R5),AC0 ;IF YELLOW, CHECK IF LESS THAN +90 DEG
CFCC
BGE .+6 ;SKIP IF LESS THAN MAX STOP LIMIT
SUBF C360,AC0 ;ELSE SUBTRACT 360 DEGREES
CMPF STOP1(R5),AC0 ;COMPARE TO MIN STOP LIMIT
CFCC
BLE J1OK ;BRANCH IF WITHIN STOP LIMITS
BR 2$
1$: CMPF STOP1(R5),AC0 ;IF BLUE, CHECK IF GREATER THAN -90 DEG
CFCC
BLE .+6 ;SKIP IF GREATER THAN
ADDF C360,AC0 ;ELSE ADD 360 DEGREES
CMPF STOP1+4(R5),AC0 ;COMPARE ANGLE TO MAX STOP LIMIT
CFCC
BGE J1OK ;BRANCH IF WITHIN STOP LIMITS
2$: BIS #JT1,EXACTS ;INDICATE NO EXACT SOLUTION
LDF STOP1(R5),AC0 ;USE CLOSEST STOP LIMIT
LDF MIDDY1(R5),AC1 ;MID-RANGE ANGLE
CMPF @(R1),AC1 ;COMPARE TO PREVIOUS
CFCC
BLE J1OK
LDF STOP1+4(R5),AC0 ;USE MAXIMUM IF CLOSER
J1OK: STF AC0,@(R1)+ ;SEND OFF NEW JOINT 1 ANGLE
JSR PC,SNCOS ;COMPUTE SIN/COS THETA 1 FOR LATER
STF AC0,ST1
STF AC1,CT1
; ["SOLVE" - JOINTS 2 & 3]
;COMPUTE JOINT 2 ANGLE
LDF SQRTXY,AC0 ;TAN TH2 = -(TX↑2+TY↑2-S2↑2)↑.5/(TZ-S1)
LDF TZ,AC1
SUBF S1(R5),AC1
JSR PC,ATAN2
STF AC0,@(R1)+ ;SEND OF NEW JOINT 2 ANGLE. NO STOP LIMIT
; CHECKING SINCE ALL SOLUTIONS WITHIN 0 TO Pi
JSR PC,SNCOS ;COMPUTE SIN/COS OF THETA 2 FOR LATER
SUBF C000,AC0 ;MAKE SURE SINE THETA 2 NOT ZERO
STF AC0,ST2
STF AC1,CT2
;COMPUTE JOINT 3 LENGTH
LDF TX,AC2 ;S3 ← (TX*CT1+TY*ST1)/ST2
MULF CT1,AC2
LDF TY,AC3
MULF ST1,AC3
ADDF AC3,AC2
DIVF AC0,AC2 ;NOW HAVE S3
CMPF STOP3+4(R5),AC2 ;COMPARE TO MAX. JOINT EXTENSION
CFCC
BGE 1$ ;BRANCH IF LESS THAN MAX
LDF STOP3+4(R5),AC2 ;ELSE SUBSTITUE MAX ALLOWABLE EXT.
BR 2$
1$: CMPF STOP3(R5),AC2 ;COMPARE TO MIN. JOINT EXTENSION
CFCC
BLE S3OK ;BRANCH IF IN RANGE
LDF STOP3(R5),AC2 ;ELSE SUBSTITUTE MIN LENGTH
2$: BIS #JT3,EXACTS ;INDICATE EXACT SOLUTION NOT FOUND
S3OK: STF AC2,@(R1)+ ;SEND BACK JOINT 3
; ["SOLVE" - JOINT 5]
;COMPUTE COS/SIN OF THETA 5 AND CHECK FOR DEGENERACY
LDF T13(R0),AC0 ;SIN TH5 = SQRT( (-T33*SN2+(T13*CT1+T23*ST1)*CT2)↑2
MULF CT1,AC0 ; + (T13*ST1-T23*CT1)↑2 )
LDF T23(R0),AC1
MULF ST1,AC1
ADDF AC1,AC0
STF AC0,T13T23 ;SAVE T13*CT1+T23*ST1
MULF CT2,AC0
LDF T33(R0),AC1
MULF ST2,AC1
SUBF AC1,AC0
STF AC0,ST4 ;THIS IS ALMOST SIN(THETA 4)
MULF AC0,AC0
LDF T13(R0),AC1 ;NOW GET T13*ST1-T23*CT1
MULF ST1,AC1
LDF T23(R0),AC2
MULF CT1,AC2
SUBF AC2,AC1
STF AC1,CT4 ;THIS IS ALMOST COS(THETA 4)
MULF AC1,AC1
ADDF AC1,AC0
JSR PC,SQRTF ;NOW HAVE SIN(THETA 5)
LDF T13T23,AC1 ;COS TH5 = T33*CT2+(T13*CT1+T23*ST1)*ST2
MULF ST2,AC1
LDF T33(R0),AC2
MULF CT2,AC2
ADDF AC2,AC1
JSR PC,ATAN2
ABSF AC0 ;WANT +180 NOT -180
TST @2(R1) ;CHECK SIGN OF PREVIOUS THETA 5
BGE .+4
NEGF AC0 ;SET SIGN OF CURRENT ANGLE THE SAME
CMPF STOP5+4(R5),AC0 ;COMPARE TO MAXIMUM STOP LIMIT
CFCC
BGE 1$ ;BRANCH IF ANGLE LESS THAN STOP LIMIT
LDF STOP5+4(R5),AC0 ;ELSE REPLACE ANGLE BY STOP LIMIT
BR 2$
1$: CMPF STOP5(R5),AC0 ;COMPARE ANGLE TO MIN STOP LIMIT
CFCC
BLE JT5OK ;BRANCH IF ANGLE IN RANGE
LDF STOP5(R5),AC0 ;ELSE REPLACE ANGLE BY MIN STOP LIMIT
2$: BIS #JT5,EXACTS ;INDICATE NO EXACT SOLUTION FOUND
JT5OK: STF AC0,@2(R1) ;SEND BACK ANGLE
STF AC0,AC1 ;CHECK FOR DEGENERATE CASE, TH5 = 0
ABSF AC1
CMPF C01,AC1
CFCC
BLT NOTDGN ;BRANCH IF NOT DEGENERATE SOLUTION
; ["SOLVE" - DEGENERATE SOLUTION AND JOINT 4]
;DEGENERATE SOLUTION: TH5 = 0, TH4 REMAINS THE SAME, SPECIAL
;EQUATION FOR TH6
LDF T31(R0),AC0 ;SIN(TH6+TH4)=-T31/ST2
LDF T32(R0),AC1 ;COS(TH6+TH4)=-T32/ST2
JSR PC,ATAN2 ;COMPUTE TH6+TH4
SUBF @(R1)+,AC0 ;TH6 = ATAN -TH4
TST (R1)+
CMPF STOP6+4(R5),AC0 ;MIGHT BE OFF BY 360 DEG
CFCC ;GREATER THAN MAXIMUM STOP LIMIT?
BGE 1$
SUBF C360,AC0 ;YES, SUBTRACT 360 DEGREES
JMP JT6CHK ;GO CHECK IF JT 6 IN RANGE
1$: CMPF STOP6(R5),AC0 ;LESS THAN MINIMUM STOP LIMIT?
CFCC
BLE 2$
ADDF C360,AC0 ;YES, ADD 360 DEGREES
2$: JMP JT6CHK
;NON-DEGENERATE SOLUTION: COMPUTE THETA 4
NOTDGN: LDF ST4,AC0 ;SIN(THETA 4)=-T33*SN2+(T13*CT1+T23*ST1)*CT2/ST5
LDF CT4,AC1 ;COS(THETA 4)=(T13*ST1-T23*CT1)/ST5
TST @2(R1) ;DONT DIVID, JUST CORRECT FOR SIGN(ST5)
BPL 1$
NEGF AC0
NEGF AC1
1$: JSR PC,ATAN2 ;COMPUTE THETA 4
STF AC0,AC1 ;DIFFERENCE BETWEEN OLD AND NEW ANGLE
SUBF @(R1),AC1
CMPF C180,AC1 ;DIFFERENCE > 180 DEG ?
CFCC
BGE 2$ ;BRANCH IF LESS
SUBF C360,AC0 ;ELSE SUBT. 360 DEG - GO THE SHORTEST DIRECTION
SUBF C360,AC1
BR 3$
2$: CMPF CM180,AC1 ;DIFFERENCE > -180 DEG ?
CFCC
BLE 3$ ;BRANCH IF GREATER
ADDF C360,AC0 ;ELSE ADD 360 DEG - GO THE SHORTEST DIRECTION
ADDF C360,AC1
3$: CMPF C90,AC1 ;DIFF. < 90 DEG ?
CFCC
BGE 4$ ;BRANCH IF IT IS
SUBF C180,AC0 ;ELSE GO THE OTHER DIRECTION AND FLIP HAND
BR FLIPJ4
4$: CMPF CM90,AC1 ;DIFF. > -90 DEG ?
CFCC
BLE JT4CHK ;BRANCH IF IT IS
ADDF C180,AC0 ;ELSE GO THE OTHER DIRECTION
FLIPJ4: NEGF @2(R1) ;FLIP HAND OVER
JT4CHK: CMPF STOP4(R5),AC0 ;COMPARE ANGLE TO MIN STOP LIMIT
CFCC
BLE 1$ ;BRANCH IF GREATER THAN MIN
ADDF C180,AC0 ;ELSE ADD 180 DEG AND FLIP HAND
BR 2$
1$: CMPF STOP4+4(R5),AC0 ;COMPARE TO MAX STOP LIMIT
CFCC
BGE JT4OK ;BRANCH IF JT 4 IN RANGE
SUBF C180,AC0 ;ELSE SUBT 180 DEG AND FLIP HAND
2$: NEGF @2(R1)
JT4OK: STF AC0,@(R1)+ ;SEND BACK THE NEW JOINT 4 ANGLE
; ["SOLVE" - JOINT 6]
;COMPUTE THETA 6
LDF ST1,AC2 ;SIN(THETA 6)=((T12*CT1+T22*ST1)*ST2
LDF CT1,AC3 ; +T32*CT2)/ST5
LDF T12(R0),AC0
MULF AC3,AC0
LDF T22(R0),AC1
MULF AC2,AC1
ADDF AC1,AC0
MULF ST2,AC0
LDF T32(R0),AC1
MULF CT2,AC1
ADDF AC1,AC0
LDF T21(R0),AC1 ;COS(THETA 6)=-((T21*ST1+T11*CT1)*ST2
MULF AC2,AC1 ; +T31*CT2)/ST5
LDF (R0),AC2
MULF AC3,AC2
ADDF AC2,AC1
MULF ST2,AC1
LDF T31(R0),AC2
MULF CT2,AC2
TST @(R1) ;DON'T DIVID BY ST5, JUST CORRECT SIGN
ADDF AC2,AC1
BGE 1$
NEGF AC0 ;IF SIN(TH5)<0 USE -SIN(TH6)
BR .+4
1$: NEGF AC1 ;ELSE USE -COS(TH6)
TST (R1)+ ;POINT TO OLD THETA 6
JSR PC,ATAN2 ;COMPUTE THETA 6
JT6CHK: CMPF STOP6(R5),AC0 ;CHECK IF ANGLE GREATER THAN MIN.
CFCC
BLE 1$ ;BRANCH IF GREATER
ADDF C180,AC0 ;ELSE ADD 180 DEG AND FLIP HAND
BR 2$
1$: CMPF STOP6+4(R5),AC0 ;CHECK IF ANGLE LESS THAN MAXIMUM
CFCC
BGE JT6OK ;BRANCH IF JT6 IN RANGE
SUBF C180,AC0 ;ELSE SUBT 180 DEG AND FLIP HAND
2$: STF AC0,@(R1) ;SAVE NEW THETA 6
NEGF @-(R1) ;FLIP HAND BY COMPLEMENTING TH5
LDF @-(R1),AC0 ;ALTER THETA 4 BY 180 DEGREES
ADDF C180,AC0 ;TRY ADDING 180 DEGREES
CMPF STOP4+4(R5),AC0 ;COMPARE TO MAXIMUM
CFCC
BGE 3$ ;BRANCH IF STILL IN RANGE
SUBF C360,AC0 ;ELSE TRY SUBT 180 DEGREES
CMPF STOP4(R5),AC0
CFCC
BLE 3$ ;BRANCH IF JT 4 NOW IN RANGE
LDF STOP4(R5),AC0 ;ELSE JUST USE STOP LIMIT
BIS #JT4,EXACTS ;INDICATE NO EXACT SOLUTION
3$: STF AC0,@(R1) ;SAVE NEW THETA 4
BR .+6
JT6OK: STF AC0,@(R1) ;SAVE NEW THETA 6
;EXIT CLEANLY
SOLDNE: MOV (SP)+,R5 ;RESTORE REGISTERS
MOV EXACTS,R0 ;LEAVE ERROR BITS FOR CALLER
RTS PC
;END OF "SOLVE"
; ["SOLVE" LOCAL STORAGE AREA]
DATA
TX: .BLKW 2 ;LOCATION OF THE END OF THE BOOM
TY: .BLKW 2
TZ: .BLKW 2
SQRTXY: .BLKW 2 ;-SQRT(TX**2+TY**2-S2**2)
T13T23: .BLKW 2 ;T13*CT1+T23*ST1
ST1: .BLKW 2 ;SIN/COS THETA 1
CT1: .BLKW 2
ST2: .BLKW 2 ;SIN/COS THETA 2
CT2: .BLKW 2
ST4: .BLKW 2 ;SIN/COS THETA 4 (ALMOST)
CT4: .BLKW 2
EXACTS: 0 ;INDICATES JTS WITH NON-EXACT SOLUTIONS
;CONSTANTS
C01: .WORD 37314,146315 ; .1000000
C001: .WORD 35603, 11157 ; .1000000@-2
C360: .WORD 42264, 0 ; 360.0000
C90: .WORD 41664, 0 ; 90.00000
CM90: .WORD 141664, 0 ;-90.00000
C180: .WORD 42064, 0 ; 180.0000
CM180: .WORD 142064, 0 ;-180.0000
C000: .WORD 31453,146167 ; .1000000@-7
CODE
;UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES
;GIVEN THE JOINT ANGLES, THE RESULTING HAND POSITION AND ORIENTATION ARE
;DETERMINED AND STORED IN A GIVEN TRANSFORM "T". THE TRANSFORM IS A 4 BY 4
;MATRIX STORED BY COLUMNS WITH EACH VALUE REPRESENTED IN TABLE COORDINATES.
;NO INITIALIZATION NEEDS TO BE DONE ON THE T SINCE ALL THE ELEMENTS WILL BE
;UPDATED, EVEN THOUGH THE LAST ROW IS ALWAYS 0,0,0,1 . A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #T,R0 ;LOAD TRANSFORM ADDRESS IN R0
; MOV #THETA,R1 ;PTR TO A TABLE CONTAINING POINTERS TO THE
; ; JOINT ANGLES FOR ALL EXISTING ARM JOINTS,
; ; I.E., THE TABLE MUST HAVE 14 ELEMENTS.
; MOV #MECHSM,R2 ;CONSTANT INDICATING WHICH ARM TO USE
; JSR PC,UPDATE ;CALLED USING PC
;
;THE TRANSFORM WILL BE OF THE FOLLOWING FORM:
;
; | T1 T5 T9 T13 |
; | T2 T6 T10 T14 |
; | T3 T7 T11 T15 |
; | 0 0 0 1.0 |
;
;THE PROCEDURE CONSISTS OF GENERATION OF TWO MATRICES T1 AND T2, THE TRANSFORM
;FROM SHOULDER TO WRIST AND FROM WRIST TO HAND RESPECTIVELY, THEN MULTIPLYING
;THE TWO TO GIVE THE DESIRED TRANSFORM.
;
;THIS PROCEDURE MODIFIED ON JUNE 9TH, 1976 TO THE EXTENT THAT THE FIRST
;COL OF T2 IS NOT COMPUTED, AND THUS ONLY THE THIRD THREE COLS OF
;T ARE COMPUTED. THE FIRST COLUMN IS THEN FOUND BY TAKING THE CROSS PRODUCT
;OF THE SECOND AND THIRD COLUMNS.
;
;IF THE BLUE OR YELLOW HAND IS SPECIFIED AS THE MECHANISM, THE HAND OPENING
;POINTED TO BY THE "THETA" ARRAY IS RETURNED IN THE LOCATION POINTED TO BY
;"T". ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT
;REPRESENATATION.
;EXECUTION TIME: 2080 microsecs
;REGISTERS USED:
;
; R0 - T TRANSFORM/LINK NUMBER
; R1 - THETA LIST/ARM CONSTANTS LIST POINTER
; R2 - BLUE/YELLOW ARM FLAG
; AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED
; R0,R1 GARBAGED
;DEFINITIONS:
SN==%0
CS==%1
SN1==%4
CS1==%5
SN2==%0
CS2==%1
SN4==%3
CS4==%4
CS5==%5
; ["UPDATE"- HAND OR T6]
UPDATE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
BIT #YELARM+BLUARM,R2 ;CHECK THAT ONE OF THE ARM REQUESTED
BNE 1$
;SECTION TO RETURN HAND OPENINGS
ADD #12.,R1 ;GET POINTER TO YELLOW HAND OPENING
BIT #YELHND,R2 ;CHECK WHICH HAND WE ARE DEALING WITH
BNE .+6 ;SKIP IF ITS THE YELLOW HAND MECH. BIT
ADD #14.,R1 ;ELSE GET THE BLUE HAND POINTER
LDF @(R1),AC0 ;GET THE PREVIOUS HAND OPENING
STF AC0,(R0) ;RETURN THE CORRECT HAND OPENING AND EXIT
BR 3$
;MUST BE AN ARM, COMPUTE T0_3 & T3_6 AND MULTIPLY THEM TOGETHER
1$: MOV #YCON,R3 ;ADDR OF YELLOW ARM PHYSICAL CONSTANTS
BIT #YELARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE 2$ ;SKIP IF YELLOW ARM
MOV #BCON,R3 ;ELSE GET ADDR OF BLUE ARM CONSTANTS
ADD #14.,R1 ;POINT TO BLUE ARM JOINT ANGLES
2$: CLR R4 ;COMPUTE T0_3 & T3_6: ADD IN BASEX,BASEY
JSR PC,T0336U
MOV #T311,R1 ;RESULT ← T3*T6
MOV #T011,R2
JSR PC,MATMUL
;RESTORE REGISTERS AND EXIT
3$: MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;END OF "UPDATE"
;JACOB - COMPUTES JACOBIAN MATRIX
;COMPUTES JACOBIAN MATRIX FOR A ARBITRARY TRANSFORMATIION.
;
; WE NEED:
; 1. PTR TO STORAGE FOR JACOBIAN MATRIX
; 2. PTR TO TABLE OF PTRS TO JOINT ANGLES
; 3. PTR TO ORC OR 6RC MATRIX
; 4. BLUE/YELLOW INDICATOR
; 5. ORC OR 6RC INDICATOR
; 6. STORAGE FOR CA6, CA3, CA0?
; R0 ← PTR TO FORCE TRANSFORMATION: C
; R1 ← PTR TO TABLE OF PTRS TO JOINT ANGLES
; R2 ← ARM/TABLE/HAND BITS
; R3 ← PTR TO ARRAY TO RETURN JACOBIAN IN
;ON COMPLETION
; T0 ← CT0 T3 ← CT3 T6 ← CT6
;EXECUTION TIME: 4 MSEC
;REGISTERS USED:
; R0,R1,R2,R3 PASS ARGUMENTS AND R0,R1 ARE GARBAGED
; ["JACOB" - FORM CT0,CT3,CT6]
JACOB: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP) ;RETURN JACOBIAN IN HERE
MOV R2,-(SP)
MOV #YCON,R3 ;ADDR OF YELLOW ARM PHYSICAL CONSTANTS
BIT #YELARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE 1$ ;SKIP IF YELLOW ARM
MOV #BCON,R3 ;ELSE GET ADDR OF BLUE ARM CONSTANTS
ADD #14.,R1 ;POINT TO BLUE ARM JOINT ANGLES
1$: MOV S1+2(R3),-(SP) ;SAVE S1
MOV S1(R3),-(SP)
MOV R0,-(SP) ;SAVE C
BIT #FTABLE,R2 ;FORCE SENSING IN HAND OR TABLE COOR.
BNE 2$
;RELATIVE HAND TRANSFORMATION GIVEN:6TC, COMPUTE 3T0 AND 6T3
JSR PC,T6330 ;T3 ← 6T3 , T6 ← 3T0
MOV #T611,-(SP) ;T6 ← CT6 ← INVERSE(6TC)
MOV #T011,-(SP) ;T0 ← CT0 ← CT3*3T0
BR GET036
;TABLE TRANSFORMATION GIVEN:0TC, COMPUTE 0T3 AND 3T6
2$: MOV #10,R4 ;T3 ← 0T3 , T6 ← 3T6
JSR PC,T0336 ;DONT ADD IN BASEX OR BASEY
MOV #T011,-(SP) ;T0 ← CT0 ← INVERSE(0TC)
MOV #T611,-(SP) ;T6 ← CT6 ← CT3*3T6
;COMPUTE T0←CT0, T3←CT3, T6←CT6
GET036: MOV R0,R1 ;C
; MOV #T311,R0 ;T3←C*T3 *K
; MOV #T311,R2 ;T3 *K
; JSR PC,MATMUL ; *K
;REMOVE FROM HERE TO
MOV #T312,R0 ;T3 ← CT3 ← C*T3
MOV #T312,R2 ;FORM COL 2,3,4 OF NEW T3
MOV #3,R3
JSR PC,MULROW
MOV #T311,R0 ;COMPUTE T3(I,1) BY CROSSING T3(I,2)
MOV #T312,R1 ; WITH T3(I,3)
MOV #T313,R2
JSR PC,CROSS
;TO HERE
MOV (SP)+,R0 ;(R0) ← T3*T6
MOV #T311,R1
MOV #T611,R2
JSR PC,MATMUL
MOV (SP)+,R0 ;(R0) ← INVERSE(C)
MOV (SP)+,R1 ;C
; JSR PC,INVERT ; *K
;REMOVE FROM HERE
MOV #3,R2
1$: MOV (R1)+,(R0)+ ;TRANSPOSE A COL INTO A ROW
MOV (R1)+,(R0)+
MOV (R1)+,10(R0)
MOV (R1)+,12(R0)
MOV (R1)+,24(R0)
MOV (R1)+,26(R0)
SOB R2,1$
ADD #30,R0
CLRF (R0)+ ;X,Y,Z ← 0
CLRF (R0)+
CLRF (R0)
;TO HERE
; ["JACOB" - POSITION VECTORS, AXES OF ROTATION]
;COMPUTE POSITION VECTORS FROM AXES OF ROTATION TO HAND AND DIRECTION
;COSINES OF AXES OF ROTATION, ALL IN COORDINATE SYSTEM C
FRMJAC: LDF (SP)+,AC3 ;NEED S1 TO DETERMINE A POINT ON Z2
MOV 2(SP),R5 ;SAVE JACOBIAN IN HERE
MOV #T614,R0 ;CPX6
MOV #T014,R1 ;CPX0
MOV #T013,R2 ;CAX0
MOV #3,R3
1$: LDF (R0)+,AC0 ;POSITION VECTOR FOR JT 1: P1 ← CP6-CP0
SUBF (R1)+,AC0
STF AC0,(R5)+
LDF (R2),AC1 ; " " " JT 2: P2 ← P1-S1*CA0
MULF AC3,AC1
MOV (R2)+,10(R5) ;ROTATION AXIS OF JT 1: Z1 ← CA0
MOV (R2)+,12(R5)
SUBF AC1,AC0
STF AC0,24(R5)
SOB R3,1$
ADD #30,R5 ;STORE ROTATION AXIS OF JT 2 HERE: Z2
MOV #T311,R0 ;Z2 ← -CT3 ( 1ST COLUMN OF MATRIX CT3 )
MOV #T313,R1 ;JT 3 JACOBIAN HAS ONLY A FORCE COMPONENT: CA3
MOV #3,R2
2$: LDF (R0)+,AC0 ;Z2 ← -CT3
NEGF AC0
STF AC0,(R5)+
LDF (R1)+,AC0 ;CA3
STF AC0,10(R5)
CLRF 24(R5) ;CONTRIBUTION OF JT3 TO ROTATIONS: Z3 ← 0
SOB R2,2$
ADD #30,R5 ;TEMPORARILY SAVE POSITION VECTOR JT 4 IN HERE
MOV #T614,R0 ;CPX6
MOV #T314,R1 ;CPX3
MOV #T313,R2 ;CAX3
MOV #3,R3
3$: LDF (R0)+,AC0 ;POSITION VECTOR FOR JT 4: P4 ← CP6-CP3
SUBF (R1)+,AC0
MOV (R2)+,14(R5) ;ROTATION AXIS OF JT 4: Z4 ← CA3
MOV (R2)+,16(R5)
STF AC0,(R5)+
STF AC0,24(R5) ;POSITION VECTOR FOR JT 5: P5 ← P4
SOB R3,3$
ADD #30,R5 ;PUT DIRECTION COSINES OF Z5 IN HERE
LDF SINT6,AC2 ;NEED SIN/COS THETA 6
LDF COST6,AC3
MOV #T611,R0 ;1ST COL OF CT6
MOV #T612,R1 ;2ND " " "
MOV #T613,R2 ;CAX6
MOV #3,R3
4$: LDF (R0)+,AC0 ;COMPONENT OF Z5 ← T6I1*ST6+T6I2*CT6
MULF AC2,AC0
; MOV 14(R2),14(R5) ;POSITION VECTOR OF JT6: P6 ← CP6 *K
; MOV 16(R2),16(R5) ; *K
CLR 14(R5) ;POSITION VECTOR FOR JT6: P6 ← 0
CLR 16(R5)
LDF (R1)+,AC1
MULF AC3,AC1
MOV (R2)+,30(R5) ;ROTATION AXIS OF JT 6: Z6 ← CAX6
MOV (R2)+,32(R5)
ADDF AC1,AC0
STF AC0,(R5)+
SOB R3,4$
; ["JACOB" - CROSS PRODUCTS, CLEAN UP]
;COMPUTE COMPONENTS OF LINEAR MOTION BY TAKING CROSS PRODUCTS. NOTE
;THAT JOINT 6 DOES NOT CONTRIBUTE TO LINEAR MOTION AND JOINT 3 NEEDS
;NO CROSS PRODUCT SINCE IT IS A PRISMATIC JOINT.
CROSSS: MOV R5,R0 ;POSITION COMPONENTS JT 1: Z1 X P1
SUB #170,R0 ;1ST ROW, 1ST COL OF JACOBIAN
MOV R0,R1
ADD #14,R1 ;Z1
MOV R0,R2 ;P1
JSR PC,CROSS
ADD #20,R0 ;POSITION COMPONENTS JT 2: Z2 X P2
ADD #20,R1 ;Z2
ADD #20,R2 ;P2
JSR PC,CROSS
ADD #50,R0 ;POSITION COMPONENTS JT 4: Z4 X P4
ADD #50,R1 ;Z4
ADD #50,R2 ;P2
JSR PC,CROSS
ADD #20,R0 ;POSITION COMPONENTS JT 5: Z5 X P5
ADD #20,R1 ;Z5
ADD #20,R2 ;P5
JSR PC,CROSS
;RESTORE REGISTERS AND EXIT
MOV (SP)+,R2 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC ;EXIT
;END OF "JACOB"
;T0336 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 1-3 AND 4-6
;"T0336" COMPUTES THE TRANSFORMATION MATRIX FROM THE BASE SYSTEM (0)
;TO THE END OF JOINT 3 AND THE MATRIX FROM THE COORDINATE SYSTEM OF
;JOINT 3 TO THE END OF JOINT 6. ON COMPLETION, THE MATRIX FROM 0 TO 3
;WILL BE STORED IN THE ARRAY "T3" AND THE MATRIX FROM 3 TO 6 WILL
;BE STORED IN "T6". IF R4 IS 0 THE X,Y LOCATION OF THE BASE OF THE
;ARM WILL BE ADDED IN, OTHERWISE R4 SHOULD BE SET TO '10
T0336U: MOV #T36PTU,R2 ;TABLE OF PTRS TO T0_3&T3_6
BR .+6
T0336: MOV #T36PTR,R2 ;TABLE OF POINTERS TO T0_3&T3_6
JSR PC,R03 ;CONSTRUCT UPPER LEFT 3x3 OF T0_3
LDF @(R1)+,AC0 ;T0_3(3,4)=CS2*S3+S1
MULF AC0,CS2
ADDF (R3)+,AC1 ;GET S1
STF AC1,T334
MULF AC0,AC2 ;T0_3(1,4)=CS1*SN2*S3-SN1*S2
MULF AC0,AC3 ;T0_3(2,4)=SN1*SN2*S3+CS1*S2
LDF (R3)+,AC1 ;GET S2
STF AC1,AC0
MULF SN1,AC0
ADD R4,R3 ;SKIP BASEX & BASEY IF REQUESTED
TST R4
BNE .+4
ADDF (R3)+,AC2 ;+ BASEX
SUBF AC0,AC2
STF AC2,T314
MULF CS1,AC1
TST R4
BNE .+4
ADDF (R3)+,AC3 ;+ BASEY
ADDF AC1,AC3
STF AC3,T324
JSR PC,R36 ;CONSTRUCT UPPER LEFT 3x3 OF T3_6
STF AC0,AC1 ;T3_6(3,4)=CS5*S6
MULF CS5,AC1
STF AC1,@(R2)+
STF AC0,AC1 ;T3_6(2,4)=SN4*ST5*S6
MULF @(R2)+,AC1
STF AC1,@(R2)+
MULF @(R2)+,AC0 ;T3_6(1,4)=CS4*ST5*S6
STF AC0,@(R2)
RTS PC
;END OF "T0336"
;T6330 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 6-4 AND 3-0
;"T6330" COMPUTES THE INVERSE TRANSFORMATION MATRICIES FROM THOSE
;COMPUTED BY "T0336". ON COMPLETION, THE MATRIX FROM JOINT 6 TO 3
;IS STORED IN "T3" AND THE MATRIX FROM 3 TO 0 IS STORED IN "T6".
;NOTE THAT THE CORRECTIONS FOR THE X-Y LOCATION OF THE BASE OF
;THE ARMS IS NOT ADDED INTO THE MATRIX TRANSFORMATIONS.
T6330: MOV #T63PTR,R2 ;TABLE OF POINTERS TO T3_0&T6_3
JSR PC,R03 ;CONSTRUCT UPPER LEFT 3X3 OF T3_0
LDF (R3)+,AC2 ;T3_0(2,4)=SN2*S1
MULF AC2,SN2
MOV (R3)+,T614 ;T3_0(1,4)=S2
STF SN2,T624
MULF AC2,CS2 ;T3_0(3,4)=-CS2*S1-S3
MOV (R3),T614+2
ADDF @(R1)+,CS2
ADD #12,R3 ;DONT ADD IN BASEX & BASEY
NEGF CS2
STF CS2,T634
JSR PC,R36 ;CONSTRUCT UPPER LEFT 3X3 OF T6_3
CLRF T314 ;T6_3(1,4)=0
CLRF T324 ;T6_3(2,4)=0
NEGF AC0 ;T6_3(3,4)=-S6
STF AC0,T334
RTS PC
;END OF "T6330"
;R03 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 1-3
;ON COMPLETION, THIS ROUTINE WILL HAVE STORED THE ELEMENTS OF R0_3(1:3,1:3)
;INTO THE ARRAY POINTED TO BY R2. IN ADDITION, THE FLOATING POINT REGISTERS
;WILL CONTAIN THE FOLLOWING VALUES:
;
; AC0= SN2 AC1= CS2 AC2= CS1*SN2
; AC3= SN1*SN2 AC4= SN1 AC5= CS1
R03: LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 1
JSR PC,SNCOS
STF SN,@(R2)+ ;T0_3(1,1)=SN1
STF CS,@(R2)+ ;T0_3(2,1)=-CS1
STF SN,SN1 ;AND SAVE FOR LATER
STF CS,CS1
LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 2
JSR PC,SNCOS
STF SN,@(R2)+ ;T0_3(3,2)=-SN2
STF CS2,@(R2)+ ;T0_3(3,3)=CS2
STF CS2,AC2 ;T0_3(1,2)=CS1*CS2
MULF CS1,AC2
ADD #100000,@-6(R2) ;CORRECT SIGN( T0_3(2,1) )
STF AC2,@(R2)+
STF CS2,AC2 ;T0_3(2,2)=SN1*CS2
MULF SN1,AC2
ADD #100000,@-6(R2) ;CORRECT SIGN( T0_3(3,2) )
STF AC2,@(R2)+
LDF SN2,AC2 ;T0_3(1,3)=CS1*SN2
MULF CS1,AC2
STF AC2,@(R2)+
LDF SN1,AC3 ;T0_3(2,3)=SN1*SN2
MULF SN2,AC3
STF AC3,@(R2)+
CLRF @(R2)+ ;T0_3(3,1)=0
RTS PC
;END OF "R03"
;R36 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 4-6
;ON COMPLETION, THIS ROUTINE WILL HAVE STORED THE ELEMENTS OF R3_6(1:3,1:3)
;INTO THE ARRAY POINTED TO BY R2. SINCE THE ELEMENT R3_6(1:1) IS NEVER
;NEED BY ANY OF THE CALLING ROUTINES, THIS IS THE ONLY MATRIX ELEMENT THAT
;IS NOT COMPUTED. AT THE COMPLETION OF THIS ROUTINE, THE FLOATING POINT
;REGISTERS CONTAIN THE FOLLOWING VALUES:
; AC0= S6 AC5= CS5
R36: LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 4
JSR PC,SNCOS
STF SN,-(SP) ;SAVE SIN/COS
STF CS,CS4
LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 5
JSR PC,SNCOS
STF CS,@(R2)+ ;T3_6(3,3)=CS5
STF SN,AC2 ;T3_6(2,3)=SN4*ST5
MULF (SP),AC2
STF AC2,@(R2)+
STF SN,AC2 ;T3_6(1,3)=CS4*ST5
MULF CS4,AC2
STF AC2,@(R2)+
STF SN,-(SP) ;SAVE SIN/COS THETA 5
STF CS,CS5
LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 6
JSR PC,SNCOS
STF SN,SINT6 ;JACOB NEEDS THESE
STF CS,COST6
LDF (SP)+,AC2 ;T3_6(3,2)=ST5*ST6
STF AC2,AC3
MULF SN,AC2
STF AC2,@(R2)+
MULF CS,AC3 ;T3_6(3,1)=-ST5*CT6
STF AC3,@(R2)
LDF (SP)+,AC2 ;NEED COMBINATIONS OF SIN/COS T4&T6
STF AC2,AC3
MULF SN,AC2 ;SN4*ST6
ADD #100000,@(R2)+ ;COMPLEMENT T3_6(3,1)
MULF CS,SN4 ;SN4*CT6
MULF CS4,SN ;CS4*ST6
MULF CS4,CS ;CS4*CT6
MULF CS5,AC2 ;T3_6(2,2)=-SN4*CS5*ST6+CS4*CT6
SUBF AC1,AC2
STF AC2,@(R2)
STF AC3,AC2 ;T3_6(2,1)=SN4*CS5*CT6+CS4*ST6
MULF CS5,AC2
ADD #100000,@(R2)+ ;COMPLEMENT T3_6(2,2)
ADDF AC0,AC2
STF AC2,@(R2)+
MULF CS5,AC0 ;T3_6(1,2)=-SN4*CT6-CS4*CS5*ST6
ADDF AC3,AC0
NEGF AC0
STF AC0,@(R2)+
LDF (R3)+,AC0 ;NEXT ROUTINE NEEDS S6
RTS PC
;END OF "R36"
;INVERT - INVERTS A TRANSFORMATION STORED BY COLUMNS
;
; R0 ← PTR TO STORAGE FOR INVERTED TRANSFORMATION
; R1 ← PTR TO TRANSFORMATION TO BE INVERTED
;
INVERT: MOV #3,R2 ;COUNTER
LDF 44.(R1),AC0
STF AC0,AC4
LDF 40.(R1),AC3
LDF 36.(R1),AC2
1$: LDF (R1)+,AC0 ;NEW T(I,4) ← -OLD T(J,I)*T(J,4)
STF AC0,(R0)
MULF AC2,AC0
ADD #12.,R0
LDF (R1)+,AC1
STF AC1,(R0)
MULF AC3,AC1
ADD #12.,R0
ADDF AC1,AC0
LDF (R1)+,AC1
STF AC1,(R0)
MULF AC4,AC1
SUB #20.,R0
ADDF AC1,AC0
NEGF AC0
STF AC0,32.(R0)
SOB R2,1$
RTS PC
;END OF "INVERT"
;MATMUL - PERFORMS THE OPERATION T0 ← T1*T2
;WON'T WORK IF LOC(T0) = LOC(T1)
;
; R0 ← PTR TO ARRAY FOR RESULT: T0
; R1 ← PTR TO T1
; R2 ← PTR TO T2
MATMUL: JSR PC,MULSHR ;FORM COL 2,3,4
SUB #36.,R0 ;COMPUTE T1(I,1) BY CROSSING T1(I,2)
MOV R0,R1 ; WITH T1(I,3)
ADD #12.,R1
MOV R1,R2
ADD #12.,R2
JSR PC,CROSS
RTS PC
MULROT: ADD #12.,R0 ;FORM COL 1,2,3,4 OF TRANS X ROTATION TRANS
ADD #12.,R2
MOV #2,R3
JSR PC,MULCOL
ADD #36.,R1 ;LAST ROW OF T0 = T1
LDF (R1)+,AC0
STF AC0,(R0)+
LDF (R1)+,AC0
STF AC0,(R0)+
LDF (R1),AC0
STF AC0,(R0)
SUB #44.,R0 ;COMPUTE T1(I,1) BY CROSSING T1(I,2)
MOV R0,R1 ; WITH T1(I,3)
ADD #12.,R1
MOV R1,R2
ADD #12.,R2
JSR PC,CROSS
RTS PC
MULSHR: ADD #12.,R0 ;FORM COL 2,3,4 OF TRANS X TRANS
ADD #12.,R2
MOV #3,R3
JSR PC,MULCOL
ADD #44.,R1 ;ADD IN T1(I,4)
ADDF (R1),AC0
STF AC0,-(R0)
LDF -(R0),AC0
ADDF -(R1),AC0
STF AC0,(R0)
LDF -(R0),AC0
ADDF -(R1),AC0
STF AC0,(R0)
RTS PC
MULR: JSR PC,MULRSH ;FORM COL 1,2,3 OF TRANS X TRANS
SUB #36.,R0 ;COMPUTE T1(I,1) BY CROSSING T1(I,2)
MOV R0,R1 ; WITH T1(I,3)
ADD #12.,R1
MOV R1,R2
ADD #12.,R2
JSR PC,CROSS
RTS PC
MULRSH: ADD #12.,R0 ;FORM COL 2,3 OF TRANS X TRANS
ADD #12.,R2
MOV #2,R3
JSR PC,MULCOL
RTS PC
;END OF "MATMUL"
;MULT - PERFORMS THE OPERATION A0 ← A1*A2
;WILL NOT WORK FOR THE CASE OF LOC(A0)=LOC(A1)
;
; R0 ← PTR TO ARRAY FOR RESULT: A0
; R1 ← PTR TO 3X3 MATRIX: A1
; R2 ← PTR TO ARRAY A2
; R3 ← NUMBER OF COLUMNS IN A0,A2
;
;AT THE END OF THE OPERATION R0,R2 ARE LEFT POINTING AT
;THE FIRST ELEMENT IN THE N+1 COLUMN
;AND AC0 CONTAINS THE LAST SUM. R1 IS LEFT UNCHANGED
MULROW: MOV #BYROWS,R4
BR MULT
MULCOL: MOV #BYCOLS,R4
MULT: MOV #3,R5 ;MULT A2 BY 3 ROWS OF A1
LDF (R2)+,AC2 ;LOAD A COLUMN OF A2
LDF (R2)+,AC3
LDF (R2)+,AC0
STF AC0,AC4
1$: LDF (R1),AC0 ;A1(I,1)*A2(1,J)
MULF AC2,AC0
ADD (R4)+,R1
LDF (R1),AC1 ;+ A1(I,2)*A2(2,J)
MULF AC3,AC1
ADD (R4)+,R1
ADDF AC1,AC0
LDF (R1),AC1 ;+ A1(I,3)*A2(3,J)
MULF AC4,AC1
ADD (R4)+,R1
MOV (R4),R4
ADDF AC1,AC0
STF AC0,(R0)+ ;SAVE A0(I,J)
SOB R5,1$
SOB R3,MULT
RTS PC
DATA
BYCOLS: .WORD 14,14,-24,.+2
.WORD 14,14,-24,.+2
.WORD 14,14,-40,BYCOLS
BYROWS: .WORD 4,4,4,.+2
.WORD 4,4,4,.+2
.WORD 4,4,-40,BYROWS
CODE
;END OF "MULT"
;CROSS - VECTOR CROSS PRODUCT OPERATION V0 ← V1 X V2
; R0 ← PTR TO VECTOR FOR RESULT: V0
; R1 ← PTR TO VECTOR V1
; R2 ← PTR TO VECTOR V2
CROSS: LDF (R1)+,AC0 ;X1
LDF (R2)+,AC1 ;X2
LDF (R2)+,AC2 ;Y2
STF AC2,AC4
LDF (R1)+,AC3 ;Y1
STF AC3,AC5
MULF AC0,AC2 ;X1*Y2
MULF AC1,AC3 ;X2*Y1
SUBF AC3,AC2 ;Z0 = X1*Y2-X2*Y1
LDF (R2),AC3 ;Z2
MULF AC3,AC0 ;X1*Z2
MULF AC5,AC3 ;Y1*Z2
STF AC0,AC5
LDF (R1),AC0 ;Z1
MULF AC0,AC1 ;Z1*X2
MULF AC4,AC0 ;Z1*Y2
SUBF AC5,AC1 ;Y0 = Z1*X2-X1*Z2
SUBF AC0,AC3 ;X0 = Y1*Z2-Z1*Y2
STF AC3,(R0)+ ;SAVE X0,Y0,Z0
STF AC1,(R0)+
STF AC2,(R0)
RTS PC
;END OF "CROSS"
;LOCAL STORAGE AREA
DATA
T36PTU: .WORD T311,T321,T332,T333,T312,T322,T313,T323,T331
.WORD T033,T023,T013,T032,T031,T022,T021,T012,T034
.WORD T023,T024,T013,T014
T36PTR: .WORD T311,T321,T332,T333,T312,T322,T313,T323,T331
.WORD T633,T623,T613,T632,T631,T622,T621,T612,T634
.WORD T623,T624,T613,T614
T63PTR: .WORD T611,T612,T623,T633,T621,T622,T631,T632,T613
.WORD T333,T332,T331,T323,T313,T322,T312,T321
T011: .BLKW 2
T021: .BLKW 2
T031: .BLKW 2
T012: .BLKW 2
T022: .BLKW 2
T032: .BLKW 2
T013: .BLKW 2
T023: .BLKW 2
T033: .BLKW 2
T014: .BLKW 2
T024: .BLKW 2
T034: .BLKW 2
T311: .BLKW 2
T321: .BLKW 2
T331: .BLKW 2
T312: .BLKW 2
T322: .BLKW 2
T332: .BLKW 2
T313: .BLKW 2
T323: .BLKW 2
T333: .BLKW 2
T314: .BLKW 2
T324: .BLKW 2
T334: .BLKW 2
CT6:
T611: .BLKW 2
T621: .BLKW 2
T631: .BLKW 2
T612: .BLKW 2
T622: .BLKW 2
T632: .BLKW 2
T613: .BLKW 2
T623: .BLKW 2
T633: .BLKW 2
T614: .BLKW 2
T624: .BLKW 2
T634: .BLKW 2
SINT6: .BLKW 2 ;SIN THETA 6
COST6: .BLKW 2 ;COS THETA 6
;PHYSICAL CONSTANTS FOR YELLOW AND BLUE ARMS
;ADDRESSES OF ARM CONSTANTS RELATIVE TO THE START OF THE FOLLOWING TABLES
S1 ==0 ;JOINT 1 OFFSET
S2 ==4 ;JOINT 2 OFFSET
BASEX ==10 ;TABLE COORDINATES OF BASE OF ARM
BASEY ==14
S6 ==20 ;JOINT 6 OFFSET
S22 ==24 ;S2**2
STOP1 ==30 ;MIN. & MAX. JOINT STOPS RELATIVE TO START OF CONST. AREA
;JOINT 2 HAS NO STOP LIMIT SINCE ALL SOLUTIONS ARE POSSIBLE
STOP3 ==40
STOP4 ==50
STOP5 ==60
STOP6 ==70
MIDDY1==100 ;MID-RANGE OF MOTION
MIDDY4==104
;CONSTANT PARAMETERS FOR YELLOW ARM, **ORDERED LIST**
YCON: .FLT2 16.24 ;JOINT 1 OFFSET
.FLT2 6.05 ;JOINT 2 OFFSET
.FLT2 29.5 ;TABLE COORDINATES OF ARM BASE
.FLT2 8.375
.FLT2 9.38 ;JOINT 6 OFFSET
.FLT2 36.6025 ;S2**2
;YELLOW ARM JOINT STOP LIMITS
.FLT2 -185.0 ;JOINT 1 MIN
.FLT2 60.0 ; MAX
.FLT2 6.5 ;JOINT 3 MIN
.FLT2 27.5 ; MAX
.FLT2 -175.0 ;JOINT 4 MIN
.FLT2 86.0 ; MAX
.FLT2 -101.0 ;JOINT 5 MIN
.FLT2 101.0 ; MAX
.FLT2 -135.0 ;JOINT 6 MIN -290.0 WITH BOTH WIPERS WORKING
.FLT2 110.0 ; MAX
.FLT2 -62.5 ;MIDDY1
.FLT2 -44.5 ;MIDDY4
; [BLUE ARM TABLE OF CONSTANTS]
;CONSTANT PARAMETERS FOR BLUE ARM, **ORDERED LIST**
BCON: .FLT2 20.24 ;JOINT 1 OFFSET
.FLT2 -6.05 ;JOINT 2 OFFSET
.FLT2 29.53125 ;TABLE COORDINATES OF ARM BASE
.FLT2 50.805
.FLT2 10.28125 ;JOINT 6 OFFSET
.FLT2 36.6025 ;S2**2
;BLUE ARM JOINT STOP LIMITS
.FLT2 -45.0 ;JOINT 1 MIN
.FLT2 190.0 ; MAX
.FLT2 6.75 ;JOINT 3 MIN
.FLT2 33.0 ; MAX
.FLT2 -395.0 ;JOINT 4 MIN
.FLT2 205.0 ; MAX
.FLT2 -95.0 ;JOINT 5 MIN
.FLT2 95.0 ; MAX
.FLT2 -110.0 ;JOINT 6 MIN
.FLT2 200.0 ; MAX
.FLT2 72.5 ;MIDDY1
.FLT2 -95.0 ;MIDDY4
;END OF "ARMSOL"